#include "PeopleGather.h"

double Get_Distance(const cv::Point a, const cv::Point b) {
    return sqrt(pow(a.x - b.x, 2) + pow(a.y - b.y, 2));
}

bool Compare_ClusterPoint(const ClusterPoint& a, const ClusterPoint& b) {
    return a.cluster < b.cluster;
}

void dbscan(std::vector<ClusterPoint>& cluster_points, double eps, int minPts) {
    int clusterId = 0;
    for (auto& cluster_point : cluster_points) {
        if (!cluster_point.visited) {
            cluster_point.visited = true;
            std::vector<ClusterPoint*> neighbors;
            // 寻找近邻
            for (auto& q : cluster_points) {
                if (!q.visited && Get_Distance(cluster_point.point_position, q.point_position) < eps) {
                    neighbors.push_back(&q);
                }
            }
            if (neighbors.size() >= minPts) {
                cluster_point.cluster = clusterId;
                // 扩展簇
                for (auto* neighbor : neighbors) {
                    if (!neighbor->visited) {
                        neighbor->visited = true;
                        std::vector<ClusterPoint*> expand_cluster_neighbors;
                        for (auto& q : cluster_points) {
                            if (!q.visited && Get_Distance(cluster_point.point_position, q.point_position) < eps) {
                                expand_cluster_neighbors.push_back(&q);
                            }
                        }
                        if (expand_cluster_neighbors.size() >= minPts) {
                            for (auto* expand_neighbor : expand_cluster_neighbors) {
                                // 检查 expand_neighbor 是否已在 neighbors 中
                                if (std::find(neighbors.begin(), neighbors.end(), expand_neighbor) == neighbors.end()) {
                                    neighbors.push_back(expand_neighbor);
                                }
                            }
                        }
                    }
                    if (neighbor->cluster == -1) {
                        neighbor->cluster = clusterId;
                    }
                }
                clusterId++;
            } else {
                cluster_point.cluster = -1;  // 离群点
            }
        }
    }
}

// 传入两个cv::point，取两个坐标值在左上的坐标
cv::Point Get_CVPoint_LeftUp(cv::Point a, cv::Point b) {
    cv::Point ans;
    int x1 = a.x, y1 = a.y;
    int x2 = b.x, y2 = b.y;
    ans.x = x1 < x2 ? x1 : x2;
    ans.y = y1 < y2 ? y1 : y2;
    return ans;
}

// 传入两个cv::point，取两个坐标值在右下的坐标
cv::Point Get_CVPoint_RightDown(cv::Point a, cv::Point b) {
    cv::Point ans;
    int x1 = a.x, y1 = a.y;
    int x2 = b.x, y2 = b.y;
    ans.x = x1 > x2 ? x1 : x2;
    ans.y = y1 > y2 ? y1 : y2;
    return ans;
}

void Get_Cluster_Rect(std::vector<std::vector<cv::Point>>& people_squeeze_rectangle, std::vector<ClusterPoint>& cluster_points, int distance_between_target, int target_around_number, int judge_peoplesqueeze_number) {
    dbscan(cluster_points, distance_between_target, target_around_number);          // 深搜，分类簇
    std::sort(cluster_points.begin(), cluster_points.end(), Compare_ClusterPoint);  // 将相同的簇放在一起
    int cluster_cnt = 0;                                                            // 计算一簇中有多少框
    cv::Point leftup_point_position = {99999, 99999};                               // 包含聚集人员的大矩形框的左上角
    cv::Point rightdown_point_position = {0, 0};                                    // 包含聚集人员的大矩形框的右下角
    // 遍历所有簇
    for (int i = 1; i < cluster_points.size(); i++) {
        if (cluster_points[i].cluster == -1) {
            cluster_cnt = i;
            continue;
        }  // 如果是离群点，跳过
        if (cluster_points[i].cluster != cluster_points[i - 1].cluster) {  // 如果发生簇变化，说明该簇统计完毕
            if (i - cluster_cnt >= judge_peoplesqueeze_number) {
                // 找到人员聚集簇啦
                people_squeeze_rectangle.push_back(std::vector<cv::Point>{leftup_point_position, rightdown_point_position});
            }
            // 一簇遍历结束，局部变量回归初始值
            leftup_point_position = {99999, 99999};
            rightdown_point_position = {0, 0};
            cluster_cnt = i;
        } else {
            // 寻找簇中最左上角的坐标
            leftup_point_position = Get_CVPoint_LeftUp(leftup_point_position, cluster_points[i].point_position);
            // 寻找簇中最右下角的坐标
            rightdown_point_position = Get_CVPoint_RightDown(rightdown_point_position, cluster_points[i].point_position);
        }
    }
}

PeopleGather::PeopleGather() {
    this->distance_between_target = 300;
    this->target_around_number = 1;
    this->judge_peoplesqueeze_number = 7;
    this->rect_id = 0;
    std::cout << "初始化PeopleGather类" << std::endl;
}

PeopleGather::PeopleGather(int distance_between_target, int target_around_number, int judge_peoplesqueeze_number) {
    this->distance_between_target = distance_between_target;
    this->target_around_number = target_around_number;
    this->judge_peoplesqueeze_number = judge_peoplesqueeze_number;
    this->rect_id = 0;
    std::cout << "初始化PeopleGather类" << std::endl;
}

void PeopleGather::Alert_Detect(std::vector<BoxInfo> detect_box) {
    // 初始化需要聚类的点
    std::vector<ClusterPoint> cluster_points;
    for (int i = 0; i < detect_box.size(); ++i) {
        int center_x = (detect_box[i].x1 + detect_box[i].x2) / 2;
        int center_y = (detect_box[i].y1 + detect_box[i].y2) / 2;
        cluster_points.push_back(ClusterPoint{cv::Point(center_x, center_y), -1, false});
    }
    // 存储人员聚集数大于阈值的矩形,二维数组形状为n*2
    std::vector<std::vector<cv::Point>> people_squeeze_rectangle;
    // 获得聚类的框
    Get_Cluster_Rect(people_squeeze_rectangle, cluster_points, this->distance_between_target, this->target_around_number, this->judge_peoplesqueeze_number);
    // 如果循环后，is_alertrect_visited元素为false，说明该帧没有发生人员聚集，人员聚集不连续，后续进行删除
    std::map<int, bool> is_alertrect_visited;
    for (auto it = this->alert_rects.begin(); it != this->alert_rects.end(); ++it)
        is_alertrect_visited[it->first] = false;
    // 合并每帧检测的框与存储类中全局的框
    for (int i = 0; i < people_squeeze_rectangle.size(); i++) {
        bool is_similar_alretrect = false;  // 判断每个检测框是否发生了合并
        for (auto it = this->alert_rects.begin(); it != this->alert_rects.end();) {
            cv::Point left_up_1 = it->second.left_up_point;
            cv::Point right_down_1 = it->second.right_down_point;
            cv::Point left_up_2 = people_squeeze_rectangle[i][0];
            cv::Point right_down_2 = people_squeeze_rectangle[i][1];

            int inner_x1 = (cv::max)(left_up_1.x, left_up_2.x);
            int inner_y1 = (cv::max)(left_up_1.y, left_up_2.y);
            int inner_x2 = (cv::min)(right_down_1.x, right_down_2.x);
            int inner_y2 = (cv::min)(right_down_1.y, right_down_2.y);
            int inner_w = (cv::max)(int(0), inner_x2 - inner_x1 + 1);
            int inner_h = (cv::max)(int(0), inner_y2 - inner_y1 + 1);
            int inner_area = inner_w * inner_h;
            int area_1 = (right_down_1.x - left_up_1.x) * (right_down_1.y - left_up_1.y);
            int area_2 = (right_down_2.x - left_up_2.x) * (right_down_2.y - left_up_2.y);
            float ovr = (float)inner_area / (area_1 + area_2 - inner_area);
            if (ovr >= 0.2) {
                is_similar_alretrect = true;
                is_alertrect_visited[it->first] = true;
                break;
            }
            it++;
        }
        if (!is_similar_alretrect) {
            cv::Point final_leftup_point_position = {people_squeeze_rectangle[i][0].x - 10, people_squeeze_rectangle[i][0].y - 10};
            cv::Point final_rightdown_point_position = {people_squeeze_rectangle[i][1].x + 10, people_squeeze_rectangle[i][1].y + 10};
            alertRect alrect_rect = {final_leftup_point_position, final_rightdown_point_position, Get_Current_Timestamp(), 0};
            this->alert_rects[this->rect_id] = alrect_rect;
            this->rect_id++;
        }
    }
    auto now_time = Get_Current_Timestamp();  // 获取当前时间戳
    for (auto it = this->alert_rects.begin(); it != this->alert_rects.end();) {
        auto visited_it = is_alertrect_visited.find(it->first);
        int squeeze_diff = int(now_time - it->second.last_gather_time) / 1000;  // 当前时间戳与连续拥挤的第一次时间戳相减 单位：秒 延迟删除
        if (visited_it != is_alertrect_visited.end() && is_alertrect_visited[it->first] == false && squeeze_diff > 2) {
            it = this->alert_rects.erase(it);
        } else {
            it++;
        }
    }
}

long long PeopleGather::Get_Current_Timestamp() {
    return std::chrono::duration_cast<std::chrono::milliseconds>(
               std::chrono::system_clock::now().time_since_epoch())
        .count();
}

void PeopleGather::Draw_All_AlertRect(cv::Mat detect_image) {
    auto now_time = Get_Current_Timestamp();  // 获取当前时间戳
    for (auto it = this->alert_rects.begin(); it != this->alert_rects.end(); ++it) {
        cv::rectangle(detect_image, it->second.left_up_point, it->second.right_down_point, cv::Scalar(0, 255, 0), 2);
        int squeeze_diff = int(now_time - it->second.last_gather_time) / 1000;
        int hours = static_cast<int>(squeeze_diff / 3600);
        int minutes = static_cast<int>((squeeze_diff % 3600) / 60);
        int seconds = static_cast<int>(squeeze_diff % 60);
        std::stringstream ss;
        ss << std::setfill('0') << std::setw(1) << hours << "h"
           << std::setw(2) << minutes << "m"
           << std::setw(2) << seconds << "s";
        std::string label = std::to_string(it->first) + " " + ss.str();
        cv::putText(detect_image, label, cv::Point(int(it->second.left_up_point.x), int(it->second.left_up_point.y) - 10), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 255, 0), 2);
    }
}

void PeopleGather::Draw_Single_AlertRect(cv::Mat detect_image, int rect_id, cv::Scalar rect_color) {
    alertRect box = this->alert_rects[rect_id];
    auto now_time = Get_Current_Timestamp();
    cv::rectangle(detect_image, box.left_up_point, box.right_down_point, rect_color, 2);
    int squeeze_diff = int(now_time - box.last_gather_time) / 1000;
    int hours = static_cast<int>(squeeze_diff / 3600);
    int minutes = static_cast<int>((squeeze_diff % 3600) / 60);
    int seconds = static_cast<int>(squeeze_diff % 60);
    std::stringstream ss;
    ss << std::setfill('0') << std::setw(1) << hours << "h"
       << std::setw(2) << minutes << "m"
       << std::setw(2) << seconds << "s";
    std::string label = std::to_string(rect_id) + " " + ss.str();
    cv::putText(detect_image, label, cv::Point(int(box.left_up_point.x), int(box.left_up_point.y) - 10), cv::FONT_HERSHEY_SIMPLEX, 1, rect_color, 2);
}

void PeopleGather::Draw_Single_AlertRect(cv::Mat detect_image, int rect_id, cv::Scalar rect_color, long long timestamp) {
    alertRect box = this->alert_rects[rect_id];
    cv::rectangle(detect_image, box.left_up_point, box.right_down_point, rect_color, 2);
    int squeeze_diff = int(timestamp - box.last_gather_time) / 1000;
    int hours = static_cast<int>(squeeze_diff / 3600);
    int minutes = static_cast<int>((squeeze_diff % 3600) / 60);
    int seconds = static_cast<int>(squeeze_diff % 60);
    std::stringstream ss;
    ss << std::setfill('0') << std::setw(1) << hours << "h"
       << std::setw(2) << minutes << "m"
       << std::setw(2) << seconds << "s";
    std::string label = std::to_string(rect_id) + " " + ss.str();
    cv::putText(detect_image, label, cv::Point(int(box.left_up_point.x), int(box.left_up_point.y) - 10), cv::FONT_HERSHEY_SIMPLEX, 1, rect_color, 2);
}
